/*
* @File Name trajectory.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\route\trajectory\trajectory.c
* @Author: Ruige_Lee
* @Date:   2019-04-22 17:09:51
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-09-24 20:45:35
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*//*
* @File Name trajectory.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\trajectory\trajectory.c
* @Author: Ruige_Lee
* @Date:   2019-04-22 10:09:40
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-04-22 16:56:26
* @Email: 295054118@whut.edu.cn"
*/



#include <stdint.h>

#include "trajectory.h"
#include "math/fast_math.h"

Cspline_Line Cspline[2];
float poseCheckArray[STEPPER_FIFO_DP][6];
uint32_t checkArrayPoint;
uint32_t checkArrayMax;


static int32_t tra_cspline_paracalc( uint8_t robot )
{
	double tempP, sig, tempQn, tempUn;

	if ( Cspline[robot].count < 3 )
	{
		return -1;
	}
	

	//head point
	{
		double delta_time;
		delta_time = Cspline[robot].points[1].timePoint - Cspline[robot].points[0].timePoint;

		for ( uint8_t axis = 0; axis < 6; axis++ )
		{
			Cspline[robot].points[0].ParaM[axis] = -0.5;
			Cspline[robot].points[0].tempU[axis] = ( 3.0 / delta_time ) 
											* (( Cspline[robot].points[1].position[axis] - Cspline[robot].points[0].position[axis] ) 
												/ delta_time );
		}

	}



	//middle point

	for ( uint16_t cnt = 1; cnt < Cspline[robot].count - 1; cnt++)
	{
		double delta_time[3];

		delta_time[0] = Cspline[robot].points[cnt + 1].timePoint - Cspline[robot].points[cnt].timePoint;
		delta_time[1] = Cspline[robot].points[cnt].timePoint - Cspline[robot].points[cnt - 1].timePoint;
		delta_time[2] = Cspline[robot].points[cnt + 1].timePoint - Cspline[robot].points[cnt - 1].timePoint;
		sig = delta_time[1] / delta_time[2];

		for ( uint8_t axis = 0; axis < 6; axis++ )
		{
			tempP = sig * Cspline[robot].points[cnt-1].ParaM[axis] + 2;
			Cspline[robot].points[cnt].ParaM[axis] = ( sig - 1.0 ) / tempP;
			Cspline[robot].points[cnt].tempU[axis] = ( Cspline[robot].points[cnt + 1].position[axis] - Cspline[robot].points[cnt].position[axis]) / delta_time[0]
															   - ( Cspline[robot].points[cnt].position[axis] - Cspline[robot].points[cnt - 1].position[axis] ) / delta_time[1];
			Cspline[robot].points[cnt].tempU[axis] =  ( 6.0 * Cspline[robot].points[cnt].tempU[axis] / delta_time[2] 
													- sig * Cspline[robot].points[cnt - 1].tempU[axis] ) 
												/ tempP;
		}
	}



	//tail point
	{
		double delta_time;

		delta_time = Cspline[robot].points[Cspline[robot].count-1].timePoint - Cspline[robot].points[Cspline[robot].count-2].timePoint;
		for ( uint8_t axis = 0; axis < 6; axis++ )
		{

			tempQn = 0.5;
			tempUn = ( 3.0 / delta_time )
					* ( 0 - 
							( Cspline[robot].points[Cspline[robot].count - 1].position[axis] - Cspline[robot].points[Cspline[robot].count - 2].position[axis] ) 
							/ delta_time );
			

			Cspline[robot].points[Cspline[robot].count - 1].ParaM[axis] = ( tempUn - tempQn * Cspline[robot].points[Cspline[robot].count - 2].tempU[axis] )
															/ ( tempQn * Cspline[robot].points[Cspline[robot].count - 2].ParaM[axis] + 1.0 );
		}

	}



	for (int16_t cnt = Cspline[robot].count - 2; cnt >= 0; cnt--)
	{
		for ( uint8_t axis = 0; axis < 6; axis++ )
		{
			Cspline[robot].points[cnt].ParaM[axis] = Cspline[robot].points[cnt].ParaM[axis] * Cspline[robot].points[cnt+1].ParaM[axis] 
												+ Cspline[robot].points[cnt].tempU[axis];
		}
	}

	return 0;
}



static int32_t tra_cspline_interpolation(uint8_t robot, double time, double* PVA)
{
	uint16_t  lowPonit, upPoint;
	double h;
	double X_Xj, X_Xk, ratio;
	double ci, di;


	//2 div to locate point

	lowPonit = 0;
	upPoint = Cspline[robot].count - 1;
	while ( upPoint-lowPonit > 1)
	{
		uint16_t divTemp;
		divTemp = ( lowPonit+upPoint ) >> 1;
		if ( Cspline[robot].points[divTemp].timePoint > time )
		{
			upPoint = divTemp;
		}

		else
		{
			lowPonit = divTemp;
		}
	}

	h = Cspline[robot].points[upPoint].timePoint - Cspline[robot].points[lowPonit].timePoint;

	if ( h == 0 )
	{
		return -1;
	}

	X_Xk = time - Cspline[robot].points[upPoint].timePoint; 
	X_Xj = time - Cspline[robot].points[lowPonit].timePoint;
	ratio = X_Xj / h;

	for ( uint8_t axis = 0; axis < 6; axis ++ )
	{
		ci = Cspline[robot].points[lowPonit].ParaM[axis] + 2 * Cspline[robot].points[upPoint].ParaM[axis];
		di = -2 * Cspline[robot].points[lowPonit].ParaM[axis] - Cspline[robot].points[upPoint].ParaM[axis];
		PVA[axis * 3 + 0] = Cspline[robot].points[lowPonit].position[axis] + ( Cspline[robot].points[upPoint].position[axis] - Cspline[robot].points[lowPonit].position[axis] ) * ratio + ( ci * X_Xj + di * X_Xk ) * ratio * X_Xk / 6.0;
		// PVA[axis * 3 + 1] = ( Cspline.points[upPoint].position[axis] - Cspline.points[lowPonit].position[axis] ) / h + 2 * ( ci + di ) * ratio * X_Xk / 6.0 + ( ci * X_Xj * X_Xj + di * X_Xk * X_Xk ) / 6.0 / h;
		// PVA[axis * 3 + 2] = ( ( ci + 2 * di ) * X_Xk + ( 2 * ci + di ) * X_Xj ) / 3 / h;
	}

	return 0;
}


//0 号机械臂检入到ping-pong buff：1号臂直接检入

int32_t tra_checkIn(uint8_t robot)
{
	float trajTime,trajTotalTime;
	double PVA[18] = {0};
	float radAngle[6];
	int16_t robotConfig[6];

	for ( uint8_t axis = 0; axis < 6; axis ++ )
	{
		radAngle[axis] = stepper_getRad( robot, axis );
	}

	stepper_enable( robot );


	if( tra_cspline_paracalc(robot) != 0 )
	{
		return -1;
	}

	trajTime = 0;
	trajTotalTime = Cspline[robot].points[Cspline[robot].count-1].timePoint;

	if ( trajTotalTime > STEPPER_FIFO_DP / 10. )
	{
		//over time flow
		return -1;
	}

	if ( robot == 0 )
	{
		checkArrayPoint = 0;
		checkArrayMax = 0;
	}

	while( trajTime <= trajTotalTime )
	{
		if( tra_cspline_interpolation(robot, trajTime, PVA) != 0 )
		{
			return -1;
		}

		if ( robot == 1 )//障碍机械臂 直接检入到执行fifo
		{
			for ( uint8_t axis = 0; axis < 6; axis++ )
			{
				
				robotConfig[axis] = stepper_rad2Pluse( axis, (float)(PVA[axis*3] - radAngle[axis]));	
				radAngle[axis] = (float)(PVA[axis*3]);
				
			}
			while( 0 != stepper_fifoPush( robot, robotConfig ) )
			{
				;
			}			
		}
		else if ( robot == 0 )//如果是控制机械臂，则需要检入整条route到数组，检查后检入
		{
			uint32_t cACnt = checkArrayMax;
			for ( uint8_t axis = 0; axis < 6; axis ++ )
			{
				poseCheckArray[cACnt][axis] = (float)(PVA[axis*3]);
			}
		
			checkArrayMax++;
			// checkArrayPoint++;
		}
		else
		{
			return -1;
		}




		trajTime = trajTime + 0.01;
	}




	return 0;
}



static float tra_minimum_time(uint8_t robot,uint8_t axis, float dtRad)
{
	// assert( dtRad > 0 );
	
	int32_t pluse;

	pluse = stepper_rad2Pluse( axis, dtRad );

	if ( robot == 0 )
	{
		return ( 1 + ( pluse / 255 ) ) * 0.01;//seconds
	}
	else 
	{	
		return ( 1 + ( pluse / 20 ) ) * 0.01;//seconds
	}
}

int32_t tra_autoSet_timePoint( uint8_t robot )
{
	int routerCnt = Cspline[robot].count;
	//load time
	Cspline[robot].points[0].timePoint = 0.01;

	for ( int pCnt = 1; pCnt < routerCnt; pCnt++ )
	{
		float maxTime = 0.;

		for ( uint8_t axis = 0; axis < 6; axis++ )
		{
			float dtRad 
				= Cspline[robot].points[pCnt].position[axis] - Cspline[robot].points[pCnt-1].position[axis];
			dtRad = fast_abs(dtRad); 
			float dtTime = tra_minimum_time(robot,axis,dtRad);
			
			maxTime = fast_max(dtTime,maxTime );

		}
		
		Cspline[robot].points[pCnt].timePoint 
			= Cspline[robot].points[pCnt-1].timePoint + maxTime + 0.01;
	}
	return 0;
}








// static int32_t poseStepper_fifoPop(float* robotConfig)
// {
// 	uint16_t FIFO_rdIndex = poseFIFO_read_prt[robot];
// 	if ( poseFIFO_empty == 1 )
// 	{
// 		robotConfig[0] = 0;
// 		robotConfig[1] = 0;
// 		robotConfig[2] = 0;
// 		robotConfig[3] = 0;
// 		robotConfig[4] = 0;
// 		robotConfig[5] = 0;
// 	}
// 	else
// 	{
// 		robotConfig[0] = poseFIFO[ FIFO_rdIndex ][0];
// 		robotConfig[1] = poseFIFO[ FIFO_rdIndex ][1];
// 		robotConfig[2] = poseFIFO[ FIFO_rdIndex ][2];
// 		robotConfig[3] = poseFIFO[ FIFO_rdIndex ][3];
// 		robotConfig[4] = poseFIFO[ FIFO_rdIndex ][4];
// 		robotConfig[5] = poseFIFO[ FIFO_rdIndex ][5];
	

// 		poseFIFO_read_prt ++;

// 		if ( poseFIFO_read_prt != STEPPER_FIFO_DP )
// 		{
// 			;
// 		}
// 		else
// 		{
// 			poseFIFO_read_prt = 0;
// 		}
// 	}

// 	if ( poseFIFO_read_prt != poseFIFO_write_prt )
// 	{
// 		poseFIFO_empty = 0;
// 	}
// 	else
// 	{
// 		poseFIFO_empty = 1;
// 	}
// 	return 0;
// }



// int32_t poseStepper_fifoPush(float* robotConfig)
// {
// 	uint16_t FIFO_wrIndex = poseFIFO_write_prt[robot];

// 	if ( ( poseFIFO_write_prt != poseFIFO_read_prt - 1 )
// 			&& ( (poseFIFO_write_prt != STEPPER_FIFO_DP - 1) || (poseFIFO_read_prt != 0) ) )
// 	{
// 		poseFIFO_full = 0;
// 	}
// 	else
// 	{
// 		poseFIFO_full = 1;
// 	}

// 	if ( poseFIFO_full == 0 )
// 	{
// 		poseFIFO[ FIFO_wrIndex ][0] = robotConfig[0];
// 		poseFIFO[ FIFO_wrIndex ][1] = robotConfig[1];
// 		poseFIFO[ FIFO_wrIndex ][2] = robotConfig[2];
// 		poseFIFO[ FIFO_wrIndex ][3] = robotConfig[3];
// 		poseFIFO[ FIFO_wrIndex ][4] = robotConfig[4];
// 		poseFIFO[ FIFO_wrIndex ][5] = robotConfig[4];

// 		poseFIFO_write_prt ++;
		

// 		if ( poseFIFO_write_prt != STEPPER_FIFO_DP )
// 		{
// 			;
// 		}
// 		else
// 		{
// 			poseFIFO_write_prt = 0;
// 		}
// 		return 0;

// 	}
// 	else
// 	{
// 		return -1;
// 	}
// }


